#include <iostream>
#include <stdio.h>
//#include "Algebra3D.h"
#include "cv.h"
#include "highgui.h"
#include "vision.h"

#define DEBUG 1

using namespace std;
int THRESHHOLD = 10;


CvCapture *capture;



float getHue(CvScalar rgb)
{
  // r,g,b values are from 0 to 1
  // h = [0,360], s = [0,1], v = [0,1]
  float r = rgb.val[2];
  float g = rgb.val[1];
  float b = rgb.val[0];
  float min, max, delta, h, s, v;
  min = MIN( r, MIN(g, b ));
  max = MAX( r, MAX(g, b ));
  v = max;
  delta = max - min;
  if( max != 0 )
    s = delta / max;
  else {
    // r = g = b = 0
    // s = 0, v is undefined
    s = 0;
    h = -1;
    return h;
  }
  if (delta == 0)
    return 360;
  if( r == max )    // between yellow & magenta
    h = ( g - b ) / delta;
  else if( g == max )  // between cyan & yellow
    h = 2 + ( b - r ) / delta;
  else  // between magenta & cyan
    h = 4 + ( r - g ) / delta;
  h *= 60;
  if( h < 0 )
    h += 360;
  return h;
}

void mapImage(IplImage* src1, IplImage* dst, float targetHue, float tolerance)
{
	CvScalar s;
	int histogram[360];
	for(int i = 0; i < 360; i++)
		histogram[i] = 0;
	int maxGreen = 0;
	int minBlue = 100000;
	float min  = 0;
	IplImage* src = cvCreateImage(cvGetSize(src1), IPL_DEPTH_8U, 3); 
	cvCvtColor( src1, src , CV_RGB2HSV);
	int nl= src->height; // number of lines
	int nc= src->width;

	for (int i=0; i<nl; i++) {
		for (int j=0; j<nc; j++) {
			// process each pixel ---------------------
			s = cvGet2D(src, i, j);
			//printf("B=%f, G=%f, R=%f\n",s.val[0],s.val[1],s.val[2]);
			//double distance = (targetColor.val[0]-s.val[0])*(targetColor.val[0]-s.val[0]) + (targetColor.val[1]-s.val[1])*(targetColor.val[1]-s.val[1]) + (targetColor.val[2]-s.val[2])*(targetColor.val[2]-s.val[2]);
			//distance /= 300;
			float hue = s.val[0];
			float difference = hue-targetHue;
			if (difference < 0)
				difference *= -1;
			if (difference > tolerance)// || s.val[2] > 210 || s.val[2] < 125)
				s.val[0] = 255;
			else{
				s.val[0] = 0;
				histogram[(int)hue]++;
			}
			//s.val[0] = hue*255/180;
			cvSet2D(dst, i, j, s);
//			if (min < s.val[0]){
			if (s.val[0] != 255 && s.val[0] != 0) {
				printf("hue=%f\n",s.val[0]);
				min = s.val[0];
			}
			// end of pixel processing ---------------
		} // end of line
	}
	cout << "Hue at 200, 260 = " << cvGet2D(src, 20, 60).val[0];
	cout << "Value at 200, 260 = " << cvGet2D(src, 20, 60).val[2];
	for(int i = 21; i < 40; i++){
		for(int j = 61; j < 80; j++)
		{
			s = cvGet2D(src, i, j);
			s.val[0] = 0;
			s.val[1] = 255;
			s.val[2] = 0;
			cvSet2D(src, i, j, s);
		}
	}
	for(int i = 0; i < 360; i++)
		if (histogram[i] > 0)
			cout << "i: " << i << "\nval: " << histogram[i]<< endl;
}

int initializeCamera()
{
	if( (capture = cvCreateCameraCapture(1)) == NULL)	{
		cout << "ERROR ON CREATING CAMERA CAPTURE!\n";
		return -1;
	}

	cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH, 640);
	cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT, 480);

	if(DEBUG){
		cvNamedWindow("Camera", CV_WINDOW_AUTOSIZE);
		cvNamedWindow("Map", CV_WINDOW_AUTOSIZE);
	}

	return 0;
}

void destroyCamera()
{
	cvReleaseCapture(&capture);
	if (DEBUG)
	{
		cvDestroyWindow("Camera");
		cvDestroyWindow("Map");
	}
}

boxCoordinates getContourInfo(CvSeq* contour)
{
	boxCoordinates b;
	b.pixelWidth = 0;
	for( int i=0; i < 10000; ++i ){
		CvPoint* p = CV_GET_SEQ_ELEM( CvPoint, contour, i );
		if (p == NULL) {/*cout << "done with contour\n";*/break;}
		for(int j = 0; j < 10000; j++){
			CvPoint* q = CV_GET_SEQ_ELEM( CvPoint, contour, j );
			if (q == NULL) {/*cout << "done with contour\n";*/break;}
			double distance = sqrt((p->x-q->x)*(p->x-q->x) + (p->y-q->y)*(p->y-q->y));
			if (distance > b.pixelWidth)
			{
				b.pixelWidth = distance;
				b.centerX = (p->x + q->x)/2;
				b.centerY = (p->y + q->y)/2;
				b.angle = atan2(p->y-q->y, p->x-q->x);
			}
		}
	}
	return b;
}

boxCoordinates getBoxCoordinates(IplImage* img_orig, int targetDimension)
{
	IplImage* img_map = cvCreateImage( cvGetSize(img_orig), IPL_DEPTH_8U, 1);
	IplImage* img_filter = cvCreateImage( cvGetSize(img_orig), 8, 1 );

	CvScalar red = CV_RGB(250,0,0);
	CvScalar blue = CV_RGB(0,0,250);

//	mapImage(img_orig, img_map, 28, 6);
	mapImage(img_orig, img_map, 13, 10);
	boxCoordinates best;

	//cvThreshold( img_8uc1, img_edge, THRESHHOLD, 255, CV_THRESH_BINARY );
	cvCopy( img_map, img_filter, NULL );
	

	CvMemStorage* storage = cvCreateMemStorage();
	CvSeq* first_contour = NULL;

	double bestDimensionScore = 1000000000;

	int Nc = cvFindContours(
			img_filter,
			storage,
			&first_contour,
			sizeof(CvContour),
			CV_RETR_LIST );

	for( CvSeq* c=first_contour; c!=NULL; c=c->h_next ){
/*		cvDrawContours(
				img_orig,
				c,
				red,		// Red
				blue,		// Blue
				1,		// Vary max_level and compare results
				2,
				8 );*/
		boxCoordinates contourCoord = getContourInfo(c);
		double dimensionScore = contourCoord.pixelWidth-targetDimension;
		if (dimensionScore < 0) dimensionScore *= -1;
	//CvPoint center = {contourCoord.centerX, contourCoord.centerY};
	//cvCircle(img_orig, center, contourCoord.pixelWidth/2, cvScalar(0,255,255));
		if (dimensionScore < bestDimensionScore){
			bestDimensionScore = dimensionScore;
			best = contourCoord;
		}
		//cvShowImage( "Camera", img_orig );
		//cvWaitKey();
	}

	//cvCvtColor( img_map, img_8uc3, CV_GRAY2BGR );
        /*
	CvPoint  curve1[]={best.X1,best.Y1,  best.X2,best.Y2,  best.X3,best.Y3,  best.X4,best.Y4};
	CvPoint* curveArr[1]={curve1};
	int      nCurvePts[1]={4};
	int      nCurves=1;
	int      isCurveClosed=1;
	int      lineWidth=1;

	cvPolyLine(img_orig,curveArr,nCurvePts,nCurves,isCurveClosed,cvScalar(0,255,255),lineWidth);*/
	CvPoint center = {best.centerX, best.centerY};
	CvPoint corner = {best.centerX + cos(best.angle)*best.pixelWidth/2, best.centerY+sin(best.angle)*best.pixelWidth/2};
	cvCircle(img_orig, center, best.pixelWidth/2, cvScalar(0,255,255));
	cvLine(img_orig, center, corner, cvScalar(0, 255, 255));

	if (DEBUG)
	{
		cvShowImage( "Camera", img_orig );
		cvShowImage( "Map", img_map);
	}
	//cvReleaseImage( &img_8uc1 );
	//cvReleaseImage( &img_orig );
	//cvReleaseImage( &img_edge );
	return best;
}

/*poDesc_t getBlockCoordinates(){
//typedef struct poDesc_t {
//    double x, y, z;
//    double theta, rho, phi;
//} poDesc_t;
	double ici[9]; //intrinsic camera inverse.
	//0 1 2
	//3 4 5
	//6 7 8
	double hec[16]; //hand eye calibration inverse
	//0 1 2 3
	//4 5 6 7
	//8 9 10 11
	//12 13 14 15
	Matrix4 hec2 = Matrix4(0, 0, 1, -4, 
				0, 1, 0, 69, 
				-1 0, 0, 77, 
				0, 0, 0, 1);
	double bgt[16]; //base to gripper transformation
	Matrix4 bgt2 = Matrix4(bgt[0], bgt[1], bgt[2], bgt[3], 
				bgt[4], bgt[5], bgt[6], bgt[7], 
				bgt[8], bgt[9], bgt[10], bgt[11], 
				bgt[12], bgt[13], bgt[15], bgt[15]);
	
	const double PROPORTION = 100; //needs to be change with experimentation.
	poDesc_t result;
	boxCoordinates b = getBoxCoordinates(cvQueryFrame(capture));
	Vector3 ray = Vector3(ici[0]* b.centerX + ici[1]*b.centerY + ici[2], 
				ici[3]* b.centerX + ici[4]*b.centerY + ici[5],
				ici[6]* b.centerX + ici[7]*b.centerY + ici[8]);
	ray = inverse(bgt2) * inverse(hec2) * coord;
	Point3 position = inverse(bgt2) * inverse(hec2) * Point3();
	double time = (100-position.z)/ray.z;
	position = position + ray*time;
	poDesc_t ret;
	ret.x = position.x;
	ret.y = position.y;
	ret.z = position.z;
	ret.rho = b.angle;
	ret.rho += 45;
	while (ret.rho < 0)
		ret.rho += 90;
	while (ret.rho > 90)
		ret.rho -= 90;
	ret.rho = ret.rho * 3.1415926/180;
	return ret;
}*/

int main()
{
  initializeCamera();
  boxCoordinates b;
  //string s = "scorbot-block/likely-close-ups/2010-12-17-183523.jpg";
  IplImage* img_orig;// = cvLoadImage(s.c_str());
  //while (1){
    img_orig = cvQueryFrame(capture);
    b = getBoxCoordinates(img_orig, 150);
    cvWaitKey();
    //char c = cvWaitKey(33);
    //if(c == 27) break;
  //}
	cout << "Center X: " << b.centerX << endl;
	cout << "Center Y: " << b.centerY << endl;
	cout << "Pixel width: " << b.pixelWidth << endl;
  //cvReleaseImage(&img_orig);
  destroyCamera();
}


